import ctypes
import time

# 加载接口库（确保ControlCANFD.dll与脚本同目录）
canDLL = ctypes.windll.LoadLibrary("./ControlCANFD.dll")

# -------------------------- 常量定义（参考手册3.1、6.2.1节及例程） --------------------------
# 设备类型（例程中USBCANFD设备类型为41，与手册6.2.1节示例一致）
VCI_USBCAN2 = 41  
# 状态码（例程中STATUS_OK=1，与库函数返回值定义匹配）
STATUS_OK = 1  
STATUS_ERR = 0
# 无效句柄（参考手册3.1节）
INVALID_DEVICE_HANDLE = 0
INVALID_CHANNEL_HANDLE = 0
# 帧类型（手册2.2节）
TYPE_CAN = 0       # 普通CAN
TYPE_CANFD = 1     # CANFD
# 滤波模式（手册3.22节）
FILTER_MODE_STD = 0  # 标准帧
FILTER_MODE_EXT = 1  # 扩展帧

# -------------------------- 数据结构定义（严格遵循手册2.2-2.8节及例程） --------------------------
# CAN通道初始化配置（手册2.2节 ZCAN_CHANNEL_INIT_CONFIG）
class _ZCAN_CHANNEL_CAN_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("acc_code", ctypes.c_uint),    # 验收码
                ("acc_mask", ctypes.c_uint),    # 屏蔽码
                ("reserved", ctypes.c_uint),    # 保留
                ("filter", ctypes.c_ubyte),     # 滤波方式（0=双滤波，1=单滤波）
                ("timing0", ctypes.c_ubyte),    # 忽略
                ("timing1", ctypes.c_ubyte),    # 忽略
                ("mode", ctypes.c_ubyte)]       # 工作模式（0=正常，1=只听）

class _ZCAN_CHANNEL_CANFD_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("acc_code", ctypes.c_uint),     # 验收码
                ("acc_mask", ctypes.c_uint),     # 屏蔽码
                ("abit_timing", ctypes.c_uint),  # 忽略
                ("dbit_timing", ctypes.c_uint),  # 忽略
                ("brp", ctypes.c_uint),          # 波特率预分频（设0）
                ("filter", ctypes.c_ubyte),      # 滤波方式
                ("mode", ctypes.c_ubyte),        # 工作模式
                ("pad", ctypes.c_ushort),        # 数据对齐
                ("reserved", ctypes.c_uint)]     # 保留

# 联合结构体（手册2.2节 union）
class _ZCAN_CHANNEL_INIT_CONFIG(ctypes.Union):
    _fields_ = [("can", _ZCAN_CHANNEL_CAN_INIT_CONFIG),
                ("canfd", _ZCAN_CHANNEL_CANFD_INIT_CONFIG)]

# 通道初始化总配置（手册2.2节）
class ZCAN_CHANNEL_INIT_CONFIG(ctypes.Structure):
    _fields_ = [("can_type", ctypes.c_uint),          # 设备类型（TYPE_CAN/TYPE_CANFD）
                ("config", _ZCAN_CHANNEL_INIT_CONFIG)] # 联合配置

# CAN帧结构（手册2.3节 can_frame）
class ZCAN_CAN_FRAME(ctypes.Structure):
    _fields_ = [("can_id", ctypes.c_uint, 29),  # 帧ID（低29位）
                ("err", ctypes.c_uint, 1),     # 错误帧标志（0=正常）
                ("rtr", ctypes.c_uint, 1),     # 远程帧标志（0=数据帧）
                ("eff", ctypes.c_uint, 1),     # 扩展帧标志（0=标准帧，1=扩展帧）
                ("can_dlc", ctypes.c_ubyte),   # 数据长度（0-8）
                ("__pad", ctypes.c_ubyte),     # 对齐
                ("__res0", ctypes.c_ubyte),    # 保留
                ("__res1", ctypes.c_ubyte),    # 保留
                ("data", ctypes.c_ubyte * 8)]  # 数据域

# 发送数据结构（手册2.5节 ZCAN_Transmit_Data）
class ZCAN_Transmit_Data(ctypes.Structure):
    _fields_ = [("frame", ZCAN_CAN_FRAME),          # CAN帧
                ("transmit_type", ctypes.c_uint)]    # 发送方式（0=正常发送）

# 接收数据结构（手册2.7节 ZCAN_Receive_Data）
class ZCAN_Receive_Data(ctypes.Structure):
    _fields_ = [("frame", ZCAN_CAN_FRAME),          # CAN帧
                ("timestamp", ctypes.c_ulonglong)]   # 时间戳（微秒）

# -------------------------- 函数参数类型声明（参考手册3.1-3.25节及例程） --------------------------
# 打开设备（手册3.1节 ZCAN_OpenDevice）
canDLL.ZCAN_OpenDevice.restype = ctypes.c_void_p
canDLL.ZCAN_OpenDevice.argtypes = (ctypes.c_uint, ctypes.c_uint, ctypes.c_uint)

# 设置CANFD仲裁域波特率（手册3.16节 ZCAN_SetAbitBaud）
canDLL.ZCAN_SetAbitBaud.restype = ctypes.c_uint
canDLL.ZCAN_SetAbitBaud.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 设置CANFD数据域波特率（手册3.17节 ZCAN_SetDbitBaud）
canDLL.ZCAN_SetDbitBaud.restype = ctypes.c_uint
canDLL.ZCAN_SetDbitBaud.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 设置CANFD标准（手册3.19节 ZCAN_SetCANFDStandard）
canDLL.ZCAN_SetCANFDStandard.restype = ctypes.c_uint
canDLL.ZCAN_SetCANFDStandard.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_ulong)

# 初始化CAN通道（手册3.5节 ZCAN_InitCAN）
canDLL.ZCAN_InitCAN.restype = ctypes.c_void_p
canDLL.ZCAN_InitCAN.argtypes = (ctypes.c_void_p, ctypes.c_ulong, ctypes.c_void_p)

# 启动CAN通道（手册3.6节 ZCAN_StartCAN）
canDLL.ZCAN_StartCAN.restype = ctypes.c_uint
canDLL.ZCAN_StartCAN.argtypes = (ctypes.c_void_p,)

# 发送CAN报文（手册3.9节 ZCAN_Transmit）
canDLL.ZCAN_Transmit.restype = ctypes.c_uint
canDLL.ZCAN_Transmit.argtypes = (ctypes.c_void_p, ctypes.c_void_p, ctypes.c_ulong)

# 获取接收缓冲区帧数（手册3.11节 ZCAN_GetReceiveNum）
canDLL.ZCAN_GetReceiveNum.restype = ctypes.c_uint
canDLL.ZCAN_GetReceiveNum.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 接收CAN报文（手册3.12节 ZCAN_Receive）
canDLL.ZCAN_Receive.restype = ctypes.c_uint
canDLL.ZCAN_Receive.argtypes = (ctypes.c_void_p, ctypes.c_void_p, ctypes.c_ulong, ctypes.c_long)

# 清除滤波配置（手册3.21节 ZCAN_ClearFilter）
canDLL.ZCAN_ClearFilter.restype = ctypes.c_uint
canDLL.ZCAN_ClearFilter.argtypes = (ctypes.c_void_p,)

# 设置滤波模式（手册3.22节 ZCAN_SetFilterMode）
canDLL.ZCAN_SetFilterMode.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterMode.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 设置滤波起始ID（手册3.23节 ZCAN_SetFilterStartID）
canDLL.ZCAN_SetFilterStartID.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterStartID.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 设置滤波结束ID（手册3.24节 ZCAN_SetFilterEndID）
canDLL.ZCAN_SetFilterEndID.restype = ctypes.c_uint
canDLL.ZCAN_SetFilterEndID.argtypes = (ctypes.c_void_p, ctypes.c_ulong)

# 生效滤波配置（手册3.25节 ZCAN_AckFilter）
canDLL.ZCAN_AckFilter.restype = ctypes.c_uint
canDLL.ZCAN_AckFilter.argtypes = (ctypes.c_void_p,)

# 复位CAN通道（手册3.7节 ZCAN_ResetCAN）
canDLL.ZCAN_ResetCAN.restype = ctypes.c_uint
canDLL.ZCAN_ResetCAN.argtypes = (ctypes.c_void_p,)

# 关闭设备（手册3.2节 ZCAN_CloseDevice）
canDLL.ZCAN_CloseDevice.restype = ctypes.c_uint
canDLL.ZCAN_CloseDevice.argtypes = (ctypes.c_void_p,)

# -------------------------- 核心功能类 --------------------------
class CANMessageSequence:
    """CAN消息序列管理类，用于发送一组消息并等待每条消息的响应"""
    
    def __init__(self, channel_handle, send_id=0x601, response_id=0x581, response_timeout=5):
        """
        初始化CAN消息序列管理器
        
        Args:
            channel_handle: CAN通道句柄
            send_id: 发送消息的ID
            response_id: 期望接收响应的ID
            response_timeout: 等待响应的超时时间（秒）
        """
        self.channel_handle = channel_handle
        self.send_id = send_id
        self.response_id = response_id
        self.response_timeout = response_timeout
        self.message_sequences = []  # 存储所有消息序列
    
    def add_sequence(self, name, messages):
        """
        添加一个消息序列
        
        Args:
            name: 序列名称
            messages: 消息列表，每个元素为包含数据的列表，例如 [0x2b, 0x40, 0x60, 0x00, 0x0f, 0x00, 0x00, 0x00]
        """
        self.message_sequences.append({
            'name': name,
            'messages': messages
        })
    
    def send_sequence(self, sequence_index):
        """
        发送指定索引的消息序列
        
        Args:
            sequence_index: 序列索引
            
        Returns:
            bool: 所有消息是否都发送成功并收到响应
        """
        if sequence_index >= len(self.message_sequences):
            print(f"❌ 序列索引 {sequence_index} 超出范围")
            return False
        
        sequence = self.message_sequences[sequence_index]
        print(f"\n🚀 开始发送序列: {sequence['name']}")
        
        for i, message_data in enumerate(sequence['messages']):
            print(f"\n📝 发送序列中的第 {i+1}/{len(sequence['messages'])} 条消息")
            
            # 发送消息
            send_success = self._send_can_data(self.send_id, message_data)
            
            if not send_success:
                print(f"❌ 发送序列中的第 {i+1} 条消息失败")
                return False
            
            # 等待响应
            print(f"⏳ 等待响应 from 0x{self.response_id:x}...")
            response_success, response_data = self._receive_can_data(self.response_id, self.response_timeout)
            
            if not response_success:
                print(f"❌ 等待第 {i+1} 条消息的响应超时")
                return False
            
            print(f"✅ 第 {i+1} 条消息处理完成")
        
        print(f"\n🎉 序列 {sequence['name']} 全部发送完成")
        return True
    
    def send_all_sequences(self):
        """
        发送所有消息序列
        
        Returns:
            bool: 所有序列是否都发送成功
        """
        for i in range(len(self.message_sequences)):
            if not self.send_sequence(i):
                print(f"❌ 序列 {i} 发送失败")
                return False
        return True
    
    def list_sequences(self):
        """列出所有已添加的消息序列"""
        print("\n📋 已配置的消息序列:")
        for i, sequence in enumerate(self.message_sequences):
            print(f"  {i}: {sequence['name']} ({len(sequence['messages'])} 条消息)")
            for j, msg in enumerate(sequence['messages']):
                print(f"    {j+1}. {[hex(d) for d in msg]}")
    
    def _send_can_data(self, send_id, data_list, is_ext_frame=False):
        """发送CAN数据（参考手册3.9节 ZCAN_Transmit）"""
        transmit_data = ZCAN_Transmit_Data()
        
        # 配置帧ID及标志位（手册2.3节 can_id格式）
        transmit_data.frame.can_id = send_id
        transmit_data.frame.eff = 1 if is_ext_frame else 0  # 扩展帧标志
        transmit_data.frame.rtr = 0  # 数据帧（非远程帧）
        transmit_data.frame.err = 0  # 正常帧
        
        # 配置数据长度及内容
        transmit_data.frame.can_dlc = len(data_list) if len(data_list) <= 8 else 8
        for i in range(transmit_data.frame.can_dlc):
            transmit_data.frame.data[i] = data_list[i]
        
        # 发送方式（0=正常发送，自动重发，手册2.5节）
        transmit_data.transmit_type = 0

        # 执行发送
        ret = canDLL.ZCAN_Transmit(self.channel_handle, ctypes.byref(transmit_data), 1)
        if ret == 1:
            print(f"\n✅ Send to 0x{send_id:x} success!")
            print(f"   DLC: {transmit_data.frame.can_dlc}, Data: {[hex(d) for d in data_list]}")
            return True
        else:
            print(f"\n❌ Send to 0x{send_id:x} failed!（返回值：{ret}）")
            return False
    
    def _receive_can_data(self, target_id, timeout=5, is_ext_frame=False):
        """接收CAN数据（参考手册3.11-3.12节）"""
        start_time = time.time()
        # 接收缓冲区（单次最大接收2500帧，参考手册例程）
        rcv_buf = (ZCAN_Receive_Data * 2500)()

        while time.time() - start_time < timeout:
            # 检查接收缓冲区帧数（0=CAN，1=CANFD，手册3.11节）
            count = canDLL.ZCAN_GetReceiveNum(self.channel_handle, TYPE_CAN)
            
            if count > 0:
                # 读取数据（wait_time=-1=无限等待，手册3.12节）
                read_len = canDLL.ZCAN_Receive(self.channel_handle, ctypes.byref(rcv_buf), count, -1)
                if read_len <= 0:
                    time.sleep(0.1)
                    continue

                # 解析接收数据
                print(f"\n✅ Received {read_len} frame(s):")
                for i in range(read_len):
                    recv_id = rcv_buf[i].frame.can_id
                    recv_eff = rcv_buf[i].frame.eff
                    # 匹配目标ID及帧类型
                    if recv_id == target_id and recv_eff == (1 if is_ext_frame else 0):
                        data = [rcv_buf[i].frame.data[j] for j in range(rcv_buf[i].frame.can_dlc)]
                        print(f"   Data: {[hex(d) for d in data]}")
                        return True, data
            time.sleep(0.1)  # 短延时降低CPU占用

        print(f"\n❌ Receive from 0x{target_id:x} timeout!（超时{timeout}秒）")
        return False, []

def init_can_device():
    """初始化CAN设备（通道0：收发0x601/0x581，参考手册5.1节流程）"""
    # 1. 打开设备（设备类型=41，索引=0，保留=0）
    m_dev = canDLL.ZCAN_OpenDevice(VCI_USBCAN2, 0, 0)
    if m_dev == INVALID_DEVICE_HANDLE:
        print("❌ Open Device failed!（检查DLL路径/设备类型/硬件连接）")
        return None, None
    print(f"✅ Open Device OK, device handle:0x{ctypes.c_uint64(m_dev).value:x}")

    # 2. 配置通道0波特率（500Kbps，参考手册3.16/3.17节）
    # 仲裁域波特率（CAN/CANFD通用）
    ret = canDLL.ZCAN_SetAbitBaud(m_dev, 0, 500000)
    if ret != STATUS_OK:
        print("❌ Set CAN0 abit:500K failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 abit:500K OK!")

    # 数据域波特率（CANFD专用，CAN可忽略但需调用）
    ret = canDLL.ZCAN_SetDbitBaud(m_dev, 0, 500000)
    if ret != STATUS_OK:
        print("❌ Set CAN0 dbit:500K failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 dbit:500K OK!")

    # 3. 设置CANFD标准（ISO模式，参考手册3.19节）
    ret = canDLL.ZCAN_SetCANFDStandard(m_dev, 0, 0)  # 0=ISO，1=BOSCH
    if ret != STATUS_OK:
        print("❌ Set CAN0 ISO mode failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Set CAN0 ISO mode OK!")

    # 4. 初始化通道0（配置为CANFD设备，参考手册2.2节）
    init_config = ZCAN_CHANNEL_INIT_CONFIG()
    init_config.can_type = TYPE_CANFD  # 按实际帧类型选择（TYPE_CAN/TYPE_CANFD）
    
    # 配置CANFD参数（基础滤波：接收所有帧）
    init_config.config.canfd.acc_code = 0x00000000    # 验收码设0
    init_config.config.canfd.acc_mask = 0xFFFFFFFF    # 屏蔽码全1=接收所有帧
    init_config.config.canfd.filter = 0               # 双滤波模式
    init_config.config.canfd.mode = 0                 # 正常工作模式
    init_config.config.canfd.brp = 0                  # 预分频设0
    init_config.config.canfd.reserved = 0             # 保留设0

    # 执行初始化
    dev_ch1 = canDLL.ZCAN_InitCAN(m_dev, 0, ctypes.byref(init_config))  # 通道0
    if dev_ch1 == INVALID_CHANNEL_HANDLE:
        print("❌ Init CAN0 failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Init CAN0 OK!")

    # 5. 配置滤波（仅接收0x581，手册3.21-3.25节，需在Start前配置）
    # 清除旧滤波
    ret = canDLL.ZCAN_ClearFilter(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Clear CAN0 filter failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 设置滤波模式（0=标准帧，1=扩展帧，按0x581实际类型选择）
    ret = canDLL.ZCAN_SetFilterMode(dev_ch1, FILTER_MODE_STD)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter mode failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 设置滤波ID范围（0x581-0x581，仅接收该ID）
    ret = canDLL.ZCAN_SetFilterStartID(dev_ch1, 0x581)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter start ID failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    ret = canDLL.ZCAN_SetFilterEndID(dev_ch1, 0x581)
    if ret != STATUS_OK:
        print("❌ Set CAN0 filter end ID failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    # 生效滤波
    ret = canDLL.ZCAN_AckFilter(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Ack CAN0 filter failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Configure CAN0 filter OK!（仅接收0x581）")

    # 6. 启动通道0
    ret = canDLL.ZCAN_StartCAN(dev_ch1)
    if ret != STATUS_OK:
        print("❌ Start CAN0 failed!")
        canDLL.ZCAN_CloseDevice(m_dev)
        return None, None
    print("✅ Start CAN0 OK!")

    return m_dev, dev_ch1

def close_can_device(m_dev, channel_handle):
    """关闭CAN设备（参考手册3.2/3.7节）"""
    # 复位通道
    if channel_handle:
        ret = canDLL.ZCAN_ResetCAN(channel_handle)
        print(f"\n{'✅ Close Channel OK!' if ret == STATUS_OK else '❌ Close Channel failed!'}")
    
    # 关闭设备
    if m_dev:
        ret = canDLL.ZCAN_CloseDevice(m_dev)
        print(f"{'✅ Close Device OK!' if ret == STATUS_OK else '❌ Close Device failed!'}")

# -------------------------- 主函数 --------------------------
if __name__ == "__main__":
    # 初始化设备（通道0）
    m_dev, dev_ch1 = init_can_device()
    if not m_dev or not dev_ch1:
        exit(1)

    try:
        # 等待设备稳定
        print("\n⌛ Waiting 2 seconds for device stabilization...")
        time.sleep(2)

        # 创建消息序列管理器
        msg_seq = CANMessageSequence(dev_ch1, send_id=0x601, response_id=0x581, response_timeout=5)
        
        Init_seq = [
            [0x2b, 0x0F, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00],
            [0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x83, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00],
            [0x23, 0x84, 0x60, 0x02, 0x64, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x06, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x07, 0x00, 0x00, 0x00],
            [0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00]
        ]
        msg_seq.add_sequence("初始化序列", Init_seq)
        
        # 示例序列2
        L_Motor_seq = [
            [0x23, 0xFF, 0x60, 0x01, 0x64, 0x00, 0x00, 0x00]
        ]
        msg_seq.add_sequence("左电机配置序列", L_Motor_seq)
        
        R_Motor_seq = [
            [0x23, 0xFF, 0x60, 0x02, 0x9C, 0xFF, 0xFF, 0xFF]
        ]
        msg_seq.add_sequence("右电机配置序列", R_Motor_seq)
        # Stop_seq = [
        #     [0x2b, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00]
        # ]
        # msg_seq.add_sequence("急停配置序列", Stop_seq)
        # # 显示所有序列
        # msg_seq.list_sequences()
        
        # 发送所有序列
        print("\n🚀 开始发送所有消息序列...")
        success = msg_seq.send_all_sequences()
        
        if success:
            print("\n🎉 所有消息序列发送完成!")
        else:
            print("\n❌ 消息序列发送过程中出现错误!")

    except KeyboardInterrupt:
        print("\n⚠️ Operation interrupted by user.")
    finally:
        # 强制清理资源
        close_can_device(m_dev, dev_ch1)